#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include "std_msgs/String.h"
#include <sstream>
#define goal_N 3
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
move_base_msgs::MoveBaseGoal goal[goal_N];
uint8_t goal_number = 100;
void dow(const std_msgs::String::ConstPtr& msg_s)
{
      ROS_INFO("%s",(*msg_s).data.c_str());
      if((*msg_s).data.c_str()=="嗯")
      {
            goal_number=3;
      }
   ;
}
int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");
    ros::init(argc, argv, "demo_simple_goal");
    ros::NodeHandle nh;
    ros::Subscriber pub = nh.subscribe("ext",10,&dow);
    //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0)))
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }
    // 第一个待发送的 目标点 在 map 坐标系下的坐标位置----客厅
  goal[0].target_pose.pose.position.x = 0;
  goal[0].target_pose.pose.position.y = 0;
  goal[0].target_pose.pose.orientation.w = 0.2;
    // 第二个待发送的 目标点 在 map 坐标系下的坐标位置----书房
  goal[1].target_pose.pose.position.x = -3;
  goal[1].target_pose.pose.position.y = 2;
  goal[1].target_pose.pose.orientation.w = 1;  
    // 第三个待发送的 目标点 在 map 坐标系下的坐标位置---
  goal[3].target_pose.pose.position.x = 3;
  goal[3].target_pose.pose.position.y =  -2;
  goal[3].target_pose.pose.orientation.w = 0.2;  
   //tell the action client that we want to spin a thread by default

        switch( (goal_N - goal_number) ){
            case 0:
                     goal[goal_N -goal_number].target_pose.header.frame_id = "map";
                     goal[goal_N-goal_number].target_pose.header.stamp = ros::Time::now();
                     ac.sendGoal(goal[goal_N -goal_number]);
                     ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
                break;
            case 1:
                     goal[goal_N -goal_number].target_pose.header.frame_id = "map";
                     goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
                     ac.sendGoal(goal[goal_N -goal_number]);
                     ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
                break;
            case 2:
                     goal[goal_N -goal_number].target_pose.header.frame_id = "map";
                     goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
                     ac.sendGoal(goal[goal_N -goal_number]);
                     ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
                break;
            case 3:
                     goal[goal_N -goal_number].target_pose.header.frame_id = "map";
                     goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
                     ac.sendGoal(goal[goal_N -goal_number]);
                     ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
                break;
            default:
                break;
        }
        ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
            ROS_INFO("The NO. %d Goal achieved success !!!", goal_N-goal_number );
            // goal_number -- ;
        }else{ROS_WARN("The NO. %d Goal Planning Failed for some reason",goal_N-goal_number); }

    ros::spin();
    return 0;
}



